/*
  IPG-50W Laser
*/

#ifndef ULAPI
#error This is intended as a userspace component only.
#endif

#ifdef DEBUG
#define DBG(fmt, ...)                   \
    do {                        \
    if (param.debug) printf(fmt,  ## __VA_ARGS__);  \
    } while(0)
#else
#define DBG(fmt, ...)
#endif

#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <string.h>
#include <ctype.h>
#include <stdlib.h>
#include <signal.h>
#include <errno.h>
#include <getopt.h>
#include <math.h>
#include <signal.h>
#include <stdarg.h>
#include <stdbool.h>

#include "rtapi.h"
#include "hal.h"
#include <modbus.h>
#include <modbus-tcp.h>
#include "inifile.h"

#include <termios.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/select.h>

// command code
//read code
#define READ_PARAM                 0x0B   //   
#define CHECK_SELF                 0x0D   //   
#define TEMPERATURE_HUMIDITY       0x10   //   temperature humidity

//Write code
#define SET_CONTROL_MODEL               0x01   //   inside 0   outside 1
#define SET_LASER_EMISSION_ON           0x04   // 
#define SET_LASER_EMISSION_OFF          0x05   //
#define SET_FREQUENCY_DUTY_CYCLE        0x06   //   Frequency Duty Cycle
#define SET_POWER_PERCENTAGE            0x07
#define SET_GUIDE_ON                    0x0E
#define SET_GUIDE_OFF                   0x0F

#define BUFFER_LENGTH               100   //

/* HAL data struct */
typedef struct
{
    //Laser output
    hal_u32_t   *status;
    hal_u32_t   *front_sample_PD;  // unit[ mV]
    hal_u32_t   *return_sample_PD; // unit[ mV]
    hal_float_t   *sample_current_1; // unit[ 0.1A]
    hal_float_t   *sample_current_2; // unit[ 0.1A]
    hal_float_t   *sample_current_3; // unit[ 0.1A]
    hal_float_t   *sample_current_4; // unit[ 0.1A]

    hal_float_t *ld_temperature_1;  // unit [0.1℃]
    hal_float_t *ld_temperature_2;  // unit [0.1℃]
    hal_float_t *ld_temperature_3;  // unit [0.1℃]
    hal_float_t *ld_temperature_4;  // unit [0.1℃]
    hal_float_t *clod_broad_temperature;// unit [0.1℃]
    hal_u32_t *power_percentage;      // unit [1%]
    hal_float_t *supply_voltage;      // unit [0.1V]
    hal_u32_t *device_temperature;    // unit [1℃]
    hal_u32_t *device_humidity;       // unit [1%]
    hal_bit_t   *modbus_ok; // the last MODBUS_OK transactions returned successfully

    //Laser input
    hal_bit_t   *control_model_cmd;    //inside 0  outside 1
    hal_bit_t   *laser_emission_cmd;   //off 0  on 1
    hal_float_t   *frequency_cmd;        // unit [Hz]
    hal_float_t   *duty_cycle_cmd;       // unit [1%]
    hal_float_t   *power_percentage_cmd; // unit [1%]
    hal_bit_t   *guide_light_cmd;      //off 0  on 1
    hal_bit_t   *enabled;

	//debug
	hal_bit_t   *laser_emission_stat;
	hal_bit_t   *laser_guidelight_stat;
	hal_bit_t   *control_model_stat;
	hal_u32_t   *power_percentage_fb;      // unit [1%]

    //
    hal_float_t looptime;
    hal_float_t speed_tolerance;



} haldata_t;

// configuration and execution state
typedef struct params
{
    int type;
    char *modname;
    int pollcycles;
    char *device;
    int baud;
    int bits;
    char parity;
    char *stopbits;
    int rts_mode;
    int serial_mode;
    struct timeval response_timeout;
    struct timeval byte_timeout;
    char *progname;
    char *section;
    FILE *fp;
    char *inifile;
    int reconnect_delay;
    int fd;
    haldata_t *haldata;
    int hal_comp_id;
    int read_initial_done;
    int old_err_reset;
    uint16_t old_cmd1_reg;      // copy of last write to FA00 */
    uint32_t modbus_ok;
    int last_errno;
    int report_device;

    volatile bool old_control_model;
    volatile bool old_emission;
    float old_frequency;        // unit [Hz]
    float old_duty_cycle;       // unit [1%]
    float old_power_percentage; // unit [1%]
    volatile bool old_guide_light;      //off 0  on 1

} params_type, *param_pointer;

#define TYPE_RTU 0
#define TYPE_TCP_SERVER 1
#define TYPE_TCP_CLIENT 2
#define TYPE_CUSTOM     3

#define POLLCYCLES  10
#define MODBUS_MIN_OK   5

int set_control_model(int fd, haldata_t *haldata, param_pointer p);
int set_guiding_light(int fd, haldata_t *haldata, param_pointer p);
int set_operating_power(int fd, haldata_t *haldata, param_pointer p);
int set_frequency_duty_cycle(int fd, haldata_t *haldata, param_pointer p);
int set_laser_emission(int fd, haldata_t *haldata, param_pointer p);
int get_read_param(int fd, haldata_t *haldata, param_pointer p);
int get_self_check(int fd, haldata_t *haldata, param_pointer p);
int get_cc_check(int fd, haldata_t *haldata, param_pointer p);
int get_device_temperatur_humidity(int fd, haldata_t *haldata, param_pointer p);
int codeing_frame(unsigned char *buffer,unsigned char *frame,int lengt);
int decodeing_frame(unsigned char *buffer,unsigned char *frame,int length);
unsigned char check_bcc(unsigned char *table,int length);
int write_data(int fd, haldata_t *haldata, param_pointer p);
int read_ini(param_pointer p);
int findkwd(param_pointer p, const char *name, int *result, const char *keyword, int value, ...);
int read_initial(int fd, haldata_t *haldata, param_pointer p);
void usage(int argc, char **argv);


static int set_baudrate(int fd,struct termios *opt, unsigned int baudrate);
static void set_stopbit(struct termios *opt, const char *stopbit);
static void set_data_bit(struct termios *opt, unsigned int databit);
static void set_parity(struct termios *opt, char parity);
static int  set_port_attr(int fd,int baudrate, int databit, const char *stopbit, char parity, int vtime,int vmin );


char stop_bits = '1';

unsigned char send_buffer[BUFFER_LENGTH] = {0};
unsigned char Recv_buffer[BUFFER_LENGTH] = {0};

struct termios old_tios;


// default options; read from inifile or command line
static params_type param =
{
    .type = -1,
    .modname = NULL,
    .pollcycles = POLLCYCLES,
    .device = "/dev/ttyS0",
    .baud = 115200,
    .bits = 8,
    .parity = 'N',
    .stopbits = &stop_bits,
    .serial_mode = -1,
    .rts_mode = -1,
    .response_timeout = { .tv_sec = 0, .tv_usec = 500000 },
    .byte_timeout = { .tv_sec = 0, .tv_usec = 500000},
    .progname = "scyg-laser",
    .section = "SCYG",
    .fp = NULL,
    .inifile = NULL,
    .reconnect_delay = 2,
    .fd = -1,
    .haldata = NULL,
    .hal_comp_id = -1,
    .read_initial_done = 0,
    .old_err_reset = 0,
    .old_cmd1_reg = 0,
    .modbus_ok = 0,    // set modbus-ok bit if last MODBUS_OK transactions went well
    .last_errno = 0,
    .report_device = 0,
    .old_control_model = true,
    .old_emission = false,
    .old_frequency = 0.0,           // unit [Hz]
    .old_duty_cycle = 0.0,          // unit [1%]
    .old_power_percentage = 0.0,    // unit [1%]
    .old_guide_light = false,     //off 0  on 1

};


static int connection_state;
enum connstate {NOT_CONNECTED, OPENING, CONNECTING, CONNECTED, RECOVER, DONE};
static bool flag = false;

static char *option_string = "dhrmn:S:I:";
static struct option long_options[] =
{
    {"debug", no_argument, 0, 'd'},
    {"help", no_argument, 0, 'h'},
    {"modbus-debug", no_argument, 0, 'm'},
    {"report-device", no_argument, 0, 'r'},
    {"ini", required_argument, 0, 'I'},     // default: getenv(INI_FILE_NAME)
    {"section", required_argument, 0, 'S'}, // default section = LIBMODBUS
    {"name", required_argument, 0, 'n'},    // vfs11_vfd
    {0,0,0,0}
};


static void  windup(param_pointer p)
{
    if (p->hal_comp_id >= 0)
        hal_exit(p->hal_comp_id);
    if (p->fd)
    {
        tcsetattr(p->fd, TCSANOW, &old_tios);
        close(p->fd);
        p->fd = -1;
    }
}

static void toggle_modbus_debug(int sig)
{
//    param.modbus_debug = !param.modbus_debug;
//    modbus_set_debug(param.ctx, param.modbus_debug);
}

static void toggle_debug(int sig)
{
//    param.debug = !param.debug;
}

static void quit(int sig)
{

    switch (connection_state)
    {

        case CONNECTING:
            // modbus_tcp_accept() or TCP modbus_connect()  were interrupted
            // these wont return to the main loop, so exit here
            windup(&param);
            exit(0);
            break;

        default:
            connection_state = DONE;
            break;
    }
}

enum kwdresult {NAME_NOT_FOUND, KEYWORD_INVALID, KEYWORD_FOUND};
#define MAX_KWD 10

int findkwd(param_pointer p, const char *name, int *result, const char *keyword, int value, ...)
{
    char *word;
    va_list ap;
    const char *kwds[MAX_KWD], **s;
    int nargs = 0;

    if ((word = iniFind(p->fp, name, p->section)) == NULL)
        return NAME_NOT_FOUND;

    kwds[nargs++] = keyword;
    va_start(ap, value);

    while (keyword != NULL)
    {
        if (!strcasecmp(word, keyword))
        {
            *result = value;
            va_end(ap);
            return KEYWORD_FOUND;
        }
        keyword = va_arg(ap, const char *);
        kwds[nargs++] = keyword;
        if (keyword)
            value = va_arg(ap, int);
    }
    fprintf(stderr, "%s: %s:[%s]%s: found '%s' - not one of: ",
            p->progname, p->inifile, p->section, name, word);
    for (s = kwds; *s; s++)
        fprintf(stderr, "%s ", *s);
    fprintf(stderr, "\n");
    va_end(ap);
    return KEYWORD_INVALID;
}

int read_ini(param_pointer p)
{
    const char *s;
//    const char *word;
    double f;
    int value;

    if ((p->fp = fopen(p->inifile,"r")) != NULL)
    {

        iniFindInt(p->fp, "BITS", p->section, &p->bits);
        iniFindInt(p->fp, "BAUD", p->section, &p->baud);
//        iniFindInt(p->fp, "TARGET", p->section, &p->slave);
        iniFindInt(p->fp, "POLLCYCLES", p->section, &p->pollcycles);
//        iniFindInt(p->fp, "PORT", p->section, &p->tcp_portno);
        iniFindInt(p->fp, "RECONNECT_DELAY", p->section, &p->reconnect_delay);
        p->stopbits = iniFind(p->fp, "STOPBITS", (const char*)(p->section));

        if ((s = iniFind(p->fp, "DEVICE", p->section)))
        {
            p->device = strdup(s);
        }
        if (iniFindDouble(p->fp, "RESPONSE_TIMEOUT", p->section, &f))
        {
            p->response_timeout.tv_sec = (int) f;
            p->response_timeout.tv_usec = (f-p->response_timeout.tv_sec) * 1000000;
        }
        if (iniFindDouble(p->fp, "BYTE_TIMEOUT", p->section, &f))
        {
            p->byte_timeout.tv_sec = (int) f;
            p->byte_timeout.tv_usec = (f-p->byte_timeout.tv_sec) * 1000000;
        }
        value = p->parity;
        if (findkwd(p, "PARITY", &value,
                    "even",'E',
                    "odd", 'O',
                    "none", 'N',
                    NULL) == KEYWORD_INVALID)
            return -1;
        p->parity = value;

        if (findkwd(p,"SERIAL_MODE", &p->serial_mode,
                    "rs232", MODBUS_RTU_RS232,
                    "rs485", MODBUS_RTU_RS485,
                    NULL) == KEYWORD_INVALID)
            return -1;

        if (findkwd(p, "TYPE", &p->type,
                    "rtu", TYPE_RTU,
                    "tcpserver", TYPE_TCP_SERVER,
                    "tcpclient", TYPE_TCP_CLIENT,
                    "custom",TYPE_CUSTOM,
                    NULL) == NAME_NOT_FOUND)
        {
            fprintf(stderr, "%s: missing required TYPE in section %s\n",
                    p->progname, p->section);
            return -1;
        }
    }
    else
    {
        fprintf(stderr, "%s:cant open inifile '%s'\n",
                p->progname, p->inifile);
        return -1;
    }
    return 0;
}

void usage(int argc, char **argv)
{
    printf("Usage:  %s [options]\n", argv[0]);
    printf("This is a userspace HAL program, typically loaded using the halcmd \"loadusr\" command:\n"
           "    loadusr vfs11_vfd [options]\n"
           "Options are:\n"
           "-I or --ini <inifile>\n"
           "    Use <inifile> (default: take ini filename from environment variable INI_FILE_NAME)\n"
           "-S or --section <section-name> (default 8)\n"
           "    Read parameters from <section_name> (default 'VFS11')\n"
           "-d or --debug\n"
           "    Turn on debugging messages. Toggled by USR1 signal.\n"
           "-m or --modbus-debug\n"
           "    Turn on modbus debugging.  This will cause all modbus messages\n"
           "    to be printed in hex on the terminal. Toggled by USR2 signal.\n"
           "-r or --report-device\n"
           "    Report device properties on console at startup\n");
}


int recv_data(int fd, void *buf, int length_to_read )
{
    int rc;
    int s_rc;
    fd_set rset;
    struct timeval tv;
    struct timeval *p_tv;
    unsigned char frame_end = 0;
    unsigned char frame_last = 0;
    int msg_length = 0;
    unsigned char *recv_data = (unsigned char *)buf;

    while ((frame_end != 0xE7)||(frame_last == 0xEE))
    {
        FD_ZERO(&rset);
        FD_SET(fd, &rset);

        tv.tv_sec = 2;
        tv.tv_usec = 0;
        p_tv = &tv;
        
        while ((s_rc = select(fd+1,&rset, NULL, NULL, p_tv)) == -1)
        {
            if (errno == EINTR)
            {

                FD_ZERO(&rset);
                FD_SET(fd, &rset);
            }
            else
            {
                return -1;
            }
        }

        if (s_rc == 0)
        {
            return -1;
        }
        
        if(FD_ISSET(fd,&rset))
        {
            rc = read(fd, recv_data + msg_length, length_to_read);
            if (rc == 0)
            {
                rc = -1;
            }

            if (rc == -1)
            {
                return -1;
            }

            /* Sums bytes received */
            msg_length += rc;
            length_to_read -= rc;
            if(msg_length > 2)
            {
                frame_end = recv_data[msg_length-1];
                frame_last = recv_data[msg_length-2];
            }
        }

    }

    return msg_length;

}


int write_data(int fd, haldata_t *haldata, param_pointer p)
{
    //waiting for Laser ready
    if (!*(haldata->enabled))
    {
        return 0;
    }

    if(*haldata->control_model_cmd != p->old_control_model)
    {
        if(set_control_model(fd,haldata,p) < 0)
            return 1;
        p->old_control_model = *haldata->control_model_cmd;
    }
    if((*haldata->frequency_cmd != p->old_frequency)||
       (*haldata->duty_cycle_cmd != p->old_duty_cycle))
    {
        if(set_frequency_duty_cycle(fd,haldata,p) < 0)
            return 1;
        p->old_frequency = *haldata->frequency_cmd;
        p->old_duty_cycle = *haldata->duty_cycle_cmd;
    }

    if(*haldata->power_percentage_cmd != p->old_power_percentage)
    {
        if(set_operating_power(fd,haldata,p) < 0)
            return 1;
        p->old_power_percentage = *haldata->power_percentage_cmd;
    }

    if(*haldata->guide_light_cmd != p->old_guide_light)
    {
        if(set_guiding_light(fd,haldata,p) < 0)
            return 1;
        p->old_guide_light = *haldata->guide_light_cmd;
    }


    if(*haldata->laser_emission_cmd != p->old_emission)
    {
        if(set_laser_emission(fd,haldata,p) < 0)
            return 1;
        p->old_emission = *haldata->laser_emission_cmd;
    }

    return 0;
}

int read_initial(int fd, haldata_t *haldata, param_pointer p)
{
    int retval = get_self_check(fd,haldata,p);
  
    if(retval < 0)
        return 1;
    usleep(10000);
    retval = get_read_param(fd,haldata,p);
    if(retval < 0)
        return 1;

    usleep(10000);
    retval = get_device_temperatur_humidity(fd,haldata,p); 
    if(retval < 0)
        return 1;
	
	//usleep(10000);
	//retval = get_cc_check(fd,haldata,p);
	//if(retval < 0)
    //    return 1;

    usleep(10000);
    return 0;

}

unsigned char check_bcc(unsigned char *table,int length)
{
    unsigned char retval = 0;
    for (int i=0; i<length; i++)
    {
        retval = retval^table[i];
    }

    return retval;
}

// buffer to send
int codeing_frame(unsigned char *buffer,unsigned char *frame,int lengt)
{
    int j = 1;

    memset(buffer,0,BUFFER_LENGTH);
    for(int i=0; i<lengt; i++)
    {
        if((frame[i] == 0x7E)||(frame[i] == 0xE7)||(frame[i] == 0xEE))
        {
            buffer[j] = 0xEE;
            j++;
        }
        buffer[j] = frame[i];
        j++;
    }

    buffer[0] = 0x7E;
    buffer[j] = 0xE7;
    j++;

    return j;

}

int decodeing_frame(unsigned char *buffer,unsigned char *frame,int length)
{
    int j = 0;

    memset(buffer,0,BUFFER_LENGTH);
//  buffer[0] = 0x7E;

    for(int i=0; i<length; i++)
    {
        if((frame[i] == 0xEE)&&
           ((frame[i+1] == 0xE7)||(frame[i+1] == 0x7E)||(frame[i+1] == 0xEE)))
        {
            continue;
        }
        buffer[j] = frame[i];
        j++;
    }
//    buffer[j] = 0xE7;
//    j++;

    return j;

}


//
int get_device_temperatur_humidity(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x10,0x10,0xA1,0xE7};
    unsigned char recv_frame[20] = {0};

    send_frame[4] = check_bcc(&send_frame[1],3);
    int size = codeing_frame(send_buffer,&send_frame[1],4);
    int len = write(fd,send_buffer,size);
    usleep(100 * size + 400);
    if(len < 0)
    {
        fprintf(stderr,"get_device_temperatur_humidity send error");
        return -1;
    }

    size = recv_data(fd, recv_frame, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur_humidity receive error");
        return -1;
    }

    int frame_len = decodeing_frame(Recv_buffer,recv_frame,size);
    unsigned char sum = check_bcc(&Recv_buffer[1],frame_len-3);
    if(sum != Recv_buffer[frame_len-2])
    {
        return -1;
    }
    if((Recv_buffer[1] != 0xA1)||(Recv_buffer[2] != 0x10))
    {
        return -1;

    }

    *haldata->device_temperature = Recv_buffer[3];
    *haldata->device_humidity = Recv_buffer[4];

    return 0;


}

int get_self_check(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x0D,0x0D,0xA1,0xE7};
    unsigned char recv_frame[20] = {0};
	
    send_frame[4] = check_bcc(&send_frame[1],3);
    int size = codeing_frame(send_buffer,&send_frame[1],4);
    int len = write(fd,send_frame,size);
    usleep(100 * size + 400);
    if(len < 0)
    {
    	if(!flag)
		{
			fprintf(stderr,"get_self_check send error");
			flag = true;
		}
        
        return -1;
    }

    size = recv_data(fd, recv_frame, 20);
    if(size < 0)
    {
    	if(!flag)
		{
			fprintf(stderr,"get_self_check receive error");
			flag = true;
		}
        
        return -1;
    }

    int frame_len = decodeing_frame(Recv_buffer,recv_frame,size);
    unsigned char sum = check_bcc(&Recv_buffer[1],frame_len-3);
    if(sum != Recv_buffer[frame_len-2])
    {
        return -1;
    }
    if((Recv_buffer[1] != 0xA1)||(Recv_buffer[2] != 0x0D))
    {
        return -1;

    }

    *haldata->status = Recv_buffer[3];
    return 0;

}

int get_cc_check(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x0C,0x0C,0xA1,0xE7};
    unsigned char recv_frame[100] = {0};

    send_frame[4] = check_bcc(&send_frame[1],3);
    int size = codeing_frame(send_buffer,&send_frame[1],4);
    int len = write(fd,send_frame,size);
    usleep(100 * size + 400);
    if(len < 0)
    {
        fprintf(stderr,"get_cc_check send error");
        return -1;
    }

    size = recv_data(fd, recv_frame, 100);
    if(size < 0)
    {
        fprintf(stderr,"get_cc_check receive error");
        return -1;
    }

    return 0;

}


int get_read_param(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x0B,0x0B,0xA1,0xE7};
    unsigned char recv_frame[100] = {0};
    uint32_t var_temp = 0;

    send_frame[4] = check_bcc(&send_frame[1],3);
    int size = codeing_frame(send_buffer,&send_frame[1],4);
    int len = write(fd,send_buffer,size);

    usleep(100 * size + 400);
    if(len < 0)
    {
        fprintf(stderr,"get_self_check send error");
        return -1;
    }

    size = recv_data(fd, recv_frame, 100);
    if(size < 0)
    {
        fprintf(stderr,"get_self_check receive error");
        return -1;
    }

    int frame_len = decodeing_frame(Recv_buffer,recv_frame,size);
    unsigned char sum = check_bcc(&Recv_buffer[1],frame_len-3);
    if(sum != Recv_buffer[frame_len-2])
    {
        return -1;
    }
    if((Recv_buffer[1] != 0xA1)||(Recv_buffer[2] != 0x0B))
    {
        return -1;
    }

    *haldata->front_sample_PD = Recv_buffer[3] <<8;
    *haldata->front_sample_PD += Recv_buffer[4];
    *haldata->return_sample_PD = Recv_buffer[5] <<8;
    *haldata->return_sample_PD += Recv_buffer[6];


    var_temp = Recv_buffer[7] <<8;
    var_temp += Recv_buffer[8];
    *haldata->sample_current_1 = 0.1 * var_temp;
    var_temp = 0;


    var_temp = Recv_buffer[9] <<8;
    var_temp += Recv_buffer[10];
    *haldata->sample_current_2 = 0.1 * var_temp;
    var_temp = 0;


    var_temp = Recv_buffer[11] <<8;
    var_temp += Recv_buffer[12];
    *haldata->sample_current_3 = 0.1 * var_temp;
    var_temp = 0;

    var_temp = Recv_buffer[13] <<8;
    var_temp+= Recv_buffer[14];
    *haldata->sample_current_4 = 0.1 * var_temp;
    var_temp = 0;


    var_temp = Recv_buffer[15] <<8;
    var_temp += Recv_buffer[16];
    *haldata->ld_temperature_1 = 0.1 * var_temp;
    var_temp = 0;

    var_temp = Recv_buffer[17] <<8;
    var_temp += Recv_buffer[18];
    *haldata->ld_temperature_2 = 0.1 * var_temp;
    var_temp = 0;

    var_temp = Recv_buffer[19] <<8;
    var_temp += Recv_buffer[20];
    *haldata->ld_temperature_3 = 0.1 * var_temp;
    var_temp = 0;

    var_temp = Recv_buffer[21] <<8;
    var_temp += Recv_buffer[22];
    *haldata->ld_temperature_4 = 0.1 * var_temp;
    var_temp = 0;


    var_temp = Recv_buffer[23] <<8;
    var_temp += Recv_buffer[24];
    *haldata->clod_broad_temperature = 0.1 * var_temp;
    var_temp = 0;

    *haldata->power_percentage = Recv_buffer[25] <<8;
    *haldata->power_percentage += Recv_buffer[26];


    var_temp = Recv_buffer[27] <<8;
    var_temp += Recv_buffer[28];
    *haldata->supply_voltage = 0.1 * var_temp;
    var_temp = 0;

    return 0;

}

//
int set_laser_emission(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x05,0x05,0xA1,0xE7};
    unsigned char recv_frame[20] = {0};

    if(*haldata->laser_emission_cmd == true)
    {
        send_frame[2] = 0x04;
        send_frame[3] = 0x04;
    }

    send_frame[4] = check_bcc(&send_frame[1],3);
    int size = codeing_frame(send_buffer,&send_frame[1],4);
    int len = write(fd,send_frame,size);
    usleep(100 * size + 400);
    if(len < 0)
    {
        fprintf(stderr,"set_laser_emission send error");
        return -1;
    }
    size = recv_data(fd, recv_frame, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_laser_emission receive error");
        return -1;
    }

    if((recv_frame[2] != send_frame[2])&&(recv_frame[3] != send_frame[3]))
    {
        return -1;
    }
	int retval = get_read_param(fd,haldata,p);
    if(retval < 0)
        return -1;

	if((recv_frame[2] == 0x04)&&(recv_frame[3] == 0x04))
    {
        *(haldata->laser_emission_stat) = true;
    }
	else if((recv_frame[2] == 0x05)&&(recv_frame[3] == 0x05))
	{
	    *(haldata->laser_emission_stat) = false;
	}
	else
	{
		fprintf(stderr,"laser emission feedback error");
	}
		

    return 0;

}

int set_frequency_duty_cycle(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x06,0x00,0x00,0x00,0x00,0xA1,0xE7};
    unsigned char recv_frame[20] = {0};

    uint32_t temp = (uint32_t)(*haldata->frequency_cmd);
    uint16_t frequency = temp;
    send_frame[3] = frequency>>8;
    send_frame[4] = frequency&0xff;
    uint32_t var_duty = (uint32_t)(*haldata->duty_cycle_cmd);
    uint16_t duty_cycle = var_duty;
    send_frame[5] = duty_cycle>>8;
    send_frame[6] = duty_cycle&0xff;

    send_frame[5] = check_bcc(&send_frame[1],6);

    int size = codeing_frame(send_buffer,&send_frame[1],7);

    int len = write(fd,send_frame,size);
    usleep(100 * size + 400);
    if(len < 0)
    {
        fprintf(stderr,"set_guiding_light send error");
        return -1;
    }
    size = recv_data(fd, recv_frame, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_guiding_light receive error");
        return -1;
    }

    if((recv_frame[2] != 0x06)&&(recv_frame[3] != 0x06))
    {
        fprintf(stderr,"set_guiding_light feedback error");
        return -1;
    }

    return 0;

}

//set operating power [%]
int set_operating_power(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x07,0x00,0x00,0xA1,0xE7};
    unsigned char recv_frame[20] = {0};

    uint32_t var_power = (uint32_t)(*haldata->power_percentage_cmd);
    uint16_t power = var_power;
    send_frame[3] = power>>8;
    send_frame[4] = power&0xff;

    send_frame[5] = check_bcc(&send_frame[1],4);

    int size = codeing_frame(send_buffer,&send_frame[1],5);

    int len = write(fd,send_frame,size);
    usleep(100 * size + 400);
    if(len < 0)
    {
        fprintf(stderr,"set_operating_power send error");
        return -1;
    }
    size = recv_data(fd, recv_frame, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_operating_power receive error");
        return -1;
    }

    if((recv_frame[2] != 0x07)&&(recv_frame[3] != 0x07))
    {
    	fprintf(stderr,"set_operating_power return error");
        return -1;
    }
	int retval = get_read_param(fd,haldata,p);
    if(retval < 0)
        return -1;
	*haldata->power_percentage_fb = var_power;

    return 0;

}

int set_guiding_light(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x0E,0x0E,0xA1,0xE7};
    unsigned char recv_frame[20] = {0};

    if(*haldata->guide_light_cmd == false)
    {
        send_frame[2] = 0x0F;
        send_frame[3] = 0x0F;
    }
    send_frame[4] = check_bcc(&send_frame[1],3);
    int size = codeing_frame(send_buffer,&send_frame[1],4);
    int len = write(fd,send_buffer,size);
    usleep(100 * size + 400);
    if(len < 0)
    {
        fprintf(stderr,"set_guiding_light send error");
        return -1;
    }
    size = recv_data(fd, recv_frame, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_guiding_light receive error");
        return -1;
    }

    if((recv_frame[2] != send_frame[2])&&(recv_frame[3] != send_frame[3]))
    {
        return -1;
    }
	int retval = get_read_param(fd,haldata,p);
    if(retval < 0)
        return -1;
	if((recv_frame[2] == 0x0E)&&(recv_frame[3] == 0x0E))
    {
        *(haldata->laser_guidelight_stat) = true;
    }
	else if((recv_frame[2] == 0x0F)&&(recv_frame[3] == 0x0F))
	{
		*(haldata->laser_guidelight_stat) = false;
	}

    return 0;

}

int set_control_model(int fd, haldata_t *haldata, param_pointer p)
{
    unsigned char send_frame[20] = {0x7E,0xA1,0x01,0x00,0xA0,0xE7};
    unsigned char recv_frame[20] = {0};

    if(*haldata->control_model_cmd == true)
    {
        send_frame[3] = 0xFF;
        send_frame[4] = 0x5F;
    }

    send_frame[4] = check_bcc(&send_frame[1],3);
    int size = codeing_frame(send_buffer,&send_frame[1],4);
    int len = write(fd,send_buffer,size);
    usleep(100 * size + 400);
    if(len < 0)
    {
        fprintf(stderr,"set_control_model send error");
        return -1;
    }
    size = recv_data(fd, recv_frame, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_control_model receive error");
        return -1;
    }

    if((recv_frame[2] != 0x01)&&(recv_frame[3] != 0x01))
    {
        fprintf(stderr,"set_control_model error 4");
        return -1;
    }
	*haldata->control_model_stat = *haldata->control_model_cmd;

    return 0;
}


static int set_baudrate(int fd,struct termios *opt, unsigned int baudrate)
{
    speed_t speed;

    switch (baudrate)
    {
        case 110:
            speed = B110;
            break;
        case 300:
            speed = B300;
            break;
        case 600:
            speed = B600;
            break;
        case 1200:
            speed = B1200;
            break;
        case 2400:
            speed = B2400;
            break;
        case 4800:
            speed = B4800;
            break;
        case 9600:
            speed = B9600;
            break;
        case 19200:
            speed = B19200;
            break;
        case 38400:
            speed = B38400;
            break;
#ifdef B57600
        case 57600:
            speed = B57600;
            break;
#endif
#ifdef B115200
        case 115200:
            speed = B115200;
            break;
#endif
#ifdef B230400
        case 230400:
            speed = B230400;
            break;
#endif
#ifdef B460800
        case 460800:
            speed = B460800;
            break;
#endif
#ifdef B500000
        case 500000:
            speed = B500000;
            break;
#endif
#ifdef B576000
        case 576000:
            speed = B576000;
            break;
#endif
#ifdef B921600
        case 921600:
            speed = B921600;
            break;
#endif
#ifdef B1000000
        case 1000000:
            speed = B1000000;
            break;
#endif
#ifdef B1152000
        case 1152000:
            speed = B1152000;
            break;
#endif
#ifdef B1500000
        case 1500000:
            speed = B1500000;
            break;
#endif
#ifdef B2500000
        case 2500000:
            speed = B2500000;
            break;
#endif
#ifdef B3000000
        case 3000000:
            speed = B3000000;
            break;
#endif
#ifdef B3500000
        case 3500000:
            speed = B3500000;
            break;
#endif
#ifdef B4000000
        case 4000000:
            speed = B4000000;
            break;
#endif
        default:
            speed = B9600;
            break;
    }

    if ((cfsetispeed(opt, speed) < 0) ||
        (cfsetospeed(opt, speed) < 0))
    {
        close(fd);
        fd = -1;
        return -1;
    }
    return  0;
}

static void set_stopbit(struct termios *opt, const char *stopbit)
{
    if (0 == strcmp (stopbit, "1"))
    {
        opt->c_cflag &= ~CSTOPB;
    }
    else if (0 == strcmp (stopbit, "1.5"))
    {
        opt->c_cflag &= ~CSTOPB;
    }
    else if (0 == strcmp (stopbit, "2"))
    {
        opt->c_cflag |= CSTOPB;
    }
    else
    {
        opt->c_cflag &= ~CSTOPB;
    }
}

static void set_data_bit(struct termios *opt, unsigned int databit)
{
    opt->c_cflag |= (CREAD | CLOCAL);
    opt->c_cflag &= ~CSIZE;
    switch (databit)
    {
        case 8:
            opt->c_cflag |= CS8;
            break;
        case 7:
            opt->c_cflag |= CS7;
            break;
        case 6:
            opt->c_cflag |= CS6;
            break;
        case 5:
            opt->c_cflag |= CS5;
            break;
        default:
            opt->c_cflag |= CS8;
            break;
    }
}

static void set_parity(struct termios *opt, char parity)
{
    switch (parity)
    {
        case 'N':
        case 'n':
            opt->c_cflag &= ~PARENB;
            break;
        case 'E':
        case 'e':
            opt->c_cflag |= PARENB;
            opt->c_cflag &= ~PARODD;
            break;
        case 'O':
        case 'o':
            opt->c_cflag |= PARENB;
            opt->c_cflag |= ~PARODD;
            break;
        default:
            opt->c_cflag &= ~PARENB;
            break;
    }
}


static int  set_port_attr(int fd,int baudrate, int databit, const char *stopbit, char parity, int vtime,int vmin )
{
    struct termios opt;
    tcgetattr(fd, &opt);

    if(set_baudrate(fd,&opt, baudrate) < 0)
    {
        return -1;

    }
    set_data_bit(&opt, databit);
    set_parity(&opt, parity);
    set_stopbit(&opt, stopbit);

//  fprintf(stderr,"baudrate %d databit %d stopbit %s parity %s",baudrate,databit,stopbit,parity);

    opt.c_cflag &= ~CRTSCTS;
    opt.c_cflag |= CLOCAL | CREAD;

    if (parity == 'N')
    {
        /* None */
        opt.c_iflag &= ~INPCK;
    }
    else
    {
        opt.c_iflag |= INPCK;
    }


    opt.c_iflag &= ~(IXON | IXOFF | IXANY| ICRNL | IGNCR);
    opt.c_oflag &= ~OPOST;

    opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    opt.c_cc[VMIN] = 0;
    opt.c_cc[VTIME] = 0;
//    tcflush (fd, TCIFLUSH);
    if (tcsetattr(fd, TCSANOW, &opt) < 0)
    {
        close(fd);
        fd = -1;
        return -1;
    }
    return 0;
}


int read_data(int fd, haldata_t *haldata, param_pointer p)
{
    int retval;
    static int Cnt = 0;

    if (!p->read_initial_done)
    {
        if ((retval = read_initial(fd, haldata, p)))
            return retval;
        else
            p->read_initial_done = 1;
    }

    // we always at least read the main status register SR_INV_OPSTATUS
    switch(Cnt)
    {
        case 0:
            retval = get_self_check(fd,haldata,p);
            Cnt++;
            break;
        case 1:
            retval = get_read_param(fd,haldata,p); 
            Cnt++;
            break;
        case 2:
            retval = get_device_temperatur_humidity(fd,haldata,p);
            Cnt++;
            break;
		case 3:
            //retval = get_self_check(fd,haldata,p);
            Cnt = 0;
            break;

    }

    if(retval  < 0)
        return 1;
    return 0;
}

#define PIN(x)                  \
    do {                        \
    status = (x);                   \
    if ((status) != 0)              \
        return status;              \
    } while (0)

int hal_setup(int id, haldata_t *h, const char *name)
{
    int status;
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->status), id, "%s.status", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->front_sample_PD), id, "%s.front-sample-PD", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->return_sample_PD), id, "%s.return-sample-PD", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->sample_current_1), id, "%s.sample-current-1", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->sample_current_2), id, "%s.sample-current-2", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->sample_current_3), id, "%s.sample-current-3", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->sample_current_4), id, "%s.sample-current-4", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->ld_temperature_1), id, "%s.ld-temperature-1", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->ld_temperature_2), id, "%s.ld-temperature-2", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->ld_temperature_3), id, "%s.ld-temperature-3", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->ld_temperature_4), id, "%s.ld-temperature-4", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->clod_broad_temperature), id, "%s.clod-broad-temperature", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->power_percentage), id, "%s.power-percentage", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->supply_voltage), id, "%s.supply-voltage", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->device_temperature), id, "%s.device-temperature", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->device_humidity), id, "%s.device-humidity", name));
    PIN(hal_pin_bit_newf(HAL_OUT, &(h->modbus_ok), id, "%s.modbus-ok", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->control_model_cmd), id, "%s.control-model-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->laser_emission_cmd), id, "%s.laser-emission-cmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->frequency_cmd), id, "%s.frequency-cmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->duty_cycle_cmd), id, "%s.duty-cycle-cmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->power_percentage_cmd), id, "%s.power-percentage-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->guide_light_cmd), id, "%s.guide-light-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->enabled), id, "%s.enabled", name));
	//debug
	PIN(hal_pin_bit_newf(HAL_IN, &(h->laser_emission_stat), id, "%s.laser_emission_stat", name));
	PIN(hal_pin_bit_newf(HAL_IN, &(h->laser_guidelight_stat), id, "%s.laser_guidelight_stat", name));
	PIN(hal_pin_bit_newf(HAL_IN, &(h->control_model_stat), id, "%s.control_model_stat", name));
	PIN(hal_pin_u32_newf(HAL_OUT, &(h->power_percentage_fb), id, "%s.power_percentage_fb", name));
    return 0;
}
#undef PIN

int set_defaults(param_pointer p)
{
    haldata_t *h = p->haldata;

    *(h->status) = 0;
    *(h->front_sample_PD) = 0;
    *(h->return_sample_PD) = 0;

    *(h->laser_emission_cmd) = false;
    *(h->guide_light_cmd) = false;
    *(h->control_model_cmd) = true; //default outside mode
    *(h->modbus_ok) = false;
    *(h->enabled) = false;

	*(h->laser_emission_stat) = false;
	*(h->laser_guidelight_stat) = false;
	*(h->control_model_stat) = true; //default outside mode
	*(h->power_percentage_fb) = 0;

    h->looptime = 0.05;
    h->speed_tolerance = 0.01;      // output frequency within 1% of target frequency
	flag = false;
// recover
//	*p->old_control_model = true;
//	*p->old_emission = false;
//	*p->old_guide_light = false;
//	*p->old_frequency = 0.0;
//	*p->old_duty_cycle = 0.0;
//	*p->old_power_percentage = 0.0;

    return 0;
}

int main(int argc, char **argv)
{
    struct timespec loop_timespec, remaining;
    int opt, socket;
    param_pointer p = &param;
    int retval = 0;
    retval = -1;
    p->progname = argv[0];
    connection_state = NOT_CONNECTED;
    p->inifile = getenv("INI_FILE_NAME");

    while ((opt = getopt_long(argc, argv, option_string, long_options, NULL)) != -1)
    {
        switch(opt)
        {
            case 'n':
                p->modname = strdup(optarg);
                break;
            case 'm':
//                p->modbus_debug = 1;
                break;
            case 'd':
//                p->debug = 1;
                break;
            case 'S':
                p->section = optarg;
                break;
            case 'I':
                p->inifile = optarg;
                break;
            case 'r':
                p->report_device = 1;
                break;
            case 'h':
            default:
//      usage(argc, argv);
                exit(0);
        }
    }

    if (p->inifile)
    {
        if (read_ini(p))
            goto finish;
        if (!p->modname)
            p->modname = "laser";
    }
    else
    {
        fprintf(stderr, "%s: ERROR: no inifile - either use '--ini inifile' or set INI_FILE_NAME environment variable\n", p->progname);
        goto finish;
    }

    signal(SIGINT, quit);
    signal(SIGTERM, quit);
    signal(SIGUSR1, toggle_debug);
    signal(SIGUSR2, toggle_modbus_debug);

    // create HAL component
    p->hal_comp_id = hal_init(p->modname);
    if ((p->hal_comp_id < 0) || (connection_state == DONE))
    {
        fprintf(stderr, "%s: ERROR: hal_init(%s) failed: HAL error code=%d\n",
                p->progname, p->modname, p->hal_comp_id);
        retval = p->hal_comp_id;
        goto finish;
    }

    // grab some shmem to store the HAL data in
    p->haldata = (haldata_t *)hal_malloc(sizeof(haldata_t));
    if ((p->haldata == 0) || (connection_state == DONE))
    {
        fprintf(stderr, "%s: ERROR: unable to allocate shared memory\n", p->modname);
        retval = -1;
        goto finish;
    }
    if (hal_setup(p->hal_comp_id,p->haldata, p->modname))
        goto finish;

    set_defaults(p);
    hal_ready(p->hal_comp_id);

    DBG("using libmodbus version %s\n", LIBMODBUS_VERSION_STRING);

    switch (p->type)
    {

        case TYPE_CUSTOM:
            connection_state = OPENING;
            p->fd = open(p->device,O_RDWR|O_NDELAY|O_NOCTTY|O_EXCL);
            if(p->fd < 0)
            {
                fprintf(stderr, "%s:%s ERROR",p->device,p->progname);
            }
            tcgetattr(p->fd, &old_tios);
            int ret = set_port_attr(p->fd,p->baud,p->bits,p->stopbits,p->parity,1,255);
            if(ret < 0)
            {
                fprintf(stderr, "%s:%s ERROR param Setting",p->device,p->progname);
            }
            sleep(p->reconnect_delay);
            tcflush(p->fd, TCIOFLUSH);//clear input and output buffer

            break;

        default:
            fprintf(stderr, "%s: ERROR: invalid connection type %d\n",
                    p->progname, p->type);
            goto finish;
    }

    connection_state = CONNECTED;
    while (connection_state != DONE)
    {

        while (connection_state == CONNECTED)
        {
            if ((retval = read_data(p->fd, p->haldata, p)))
            {
                p->modbus_ok = 0;
            }
            else
            {
                p->modbus_ok++;
            }
            if (p->modbus_ok > MODBUS_MIN_OK)
            {
                *(p->haldata->modbus_ok) = 1;
            }
            else
            {
                *(p->haldata->modbus_ok) = 0;
            }
            if ((retval = write_data(p->fd, p->haldata, p)))
            {
                p->modbus_ok = 0;
                if (retval == 1)
                {
                    connection_state = RECOVER;
                }
            }
            else
            {
                p->modbus_ok++;
            }
            if (p->modbus_ok > MODBUS_MIN_OK)
            {
                *(p->haldata->modbus_ok) = 1;
            }
            else
            {
                *(p->haldata->modbus_ok) = 0;
            }
            /* don't want to scan too fast, and shouldn't delay more than a few seconds */
            if (p->haldata->looptime < 0.001) p->haldata->looptime = 0.001;
            if (p->haldata->looptime > 1.0) p->haldata->looptime = 1.0;
            loop_timespec.tv_sec = (time_t)(p->haldata->looptime);
            loop_timespec.tv_nsec = (long)((p->haldata->looptime - loop_timespec.tv_sec) * 1000000000l);
            nanosleep(&loop_timespec, &remaining);
        }

        switch (connection_state)
        {
            case DONE:
                // cleanup actions before exiting.
                tcflush(p->fd, TCIOFLUSH);//clear input and output buffer
                DBG("cleanup actions before exiting");
                break;

            case RECOVER:
                DBG("recover\n");
                set_defaults(p);
                p->read_initial_done = 0;
                // reestablish connection to slave
                switch (p->type)
                {

                    case TYPE_RTU:
                    case TYPE_TCP_CLIENT:
                        DBG("rtu/tcp reconnect\n");
                        break;

                    case TYPE_CUSTOM:
                        ioctl(p->fd, TCFLSH, 2);  //clear input and output buffer
                        close(p->fd);
                        p->fd = -1;
                        while ((connection_state != CONNECTED) &&
                               (connection_state != DONE))
                        {
                            sleep(p->reconnect_delay);
                             p->fd = open(p->device,O_RDWR|O_NDELAY|O_NOCTTY|O_EXCL);
                            if(p->fd < 0)
                            {
                                fprintf(stderr, "%s:%s ERROR",p->device,p->progname);
                            }
                            else
                            {
                                connection_state = CONNECTED;
                                tcgetattr(p->fd, &old_tios);
                                int ret = set_port_attr(p->fd,p->baud,p->bits,p->stopbits,p->parity,1,100);
                                if(ret < 0)
                                {
                                    fprintf(stderr, "%s:%s ERROR param Setting",p->device,p->progname);
                                }
                                sleep(p->reconnect_delay);
                                tcflush(p->fd, TCIOFLUSH);//clear input and output buffer
                                DBG("custom reconnect\n");
                            }
                            
                        }
                        break;

                    case TYPE_TCP_SERVER:
                        DBG("tcpserver reconnect\n");
                        break;

                    default:
                        break;
                }
                break;
            default:
                ;
                break;
        }
    }
    retval = 0;

finish:
    windup(p);
    return retval;
}



